//
// Created by xiang on 2022/3/15.
//
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/highgui.hpp>

#include "ch6/icp_2d.h"
#include "ch6/lidar_2d_utils.h"
#include "common/io_utils.h"

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
//DEFINE_string(method, "point2point", "2d icp方法：point2point/point2plane");
DEFINE_string(method, "point2plane", "2d icp方法：point2point/point2plane");
/// 测试从rosbag中读取2d scan并plot的结果
/// 通过选择method来确定使用点到点或点到面的ICP
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);

    Scan2dPtr last_scan = nullptr, current_scan = nullptr;
    io.SetScan2DProcessFunc([&](const Scan2d& scan) {
        current_scan = std::make_shared<Scan2d>(scan);

        if (last_scan == nullptr) {
            last_scan = current_scan;
            return true;
        }

        sad::Icp2d icp;
        icp.SetTarget(last_scan);
        icp.SetSource(current_scan);

        SE2 pose;
        if (fLS::FLAGS_method == "point2point") {
            icp.AlignGaussNewton(pose);
        } else if (fLS::FLAGS_method == "point2plane") {
            icp.AlignGaussNewtonPoint2Plane(pose);
        }

        cv::Mat image;
        sad::Visualize2DScan(last_scan, SE2(), image, Vec3b(255, 0, 0));    // target是蓝的
        sad::Visualize2DScan(current_scan, pose, image, Vec3b(0, 0, 255));  // source是红的
        cv::imshow("scan", image);
        cv::waitKey(20);

        last_scan = current_scan;
        return true;
    }).Go();

    return 0;
}